#include <QByteArray>
#include <QXmlAttributes>
#include <QXmlStreamReader>
#include <QString>
#include <QDebug>
#include <QDateTime>

#include "_Interface/_global_datastruct.h"
#include "Backend/bjutrfswtech.h"

//note： 向下工具侧是正
BasicRFSWTech::BasicRFSWTech(){
    this->nowRFSStage = InitStage;
    this->StaticTimeKey = 0;
    this->DynamicTimekey = 0;
}

BasicRFSWTech::~BasicRFSWTech(){

}

short BasicRFSWTech::GetRobobtandFswParamaters(ST_RobotDAQData RobotDAQData, ST_FswTecDAQData FswTecDAQData){
    this->mRobotDAQData = RobotDAQData;
    this->mFswTecDAQData = FswTecDAQData;
    return 1;
}

short BasicRFSWTech::ReturnRSFSWStage(short &stage){
    stage = nowRFSStage;
    return 1;
}

short BasicRFSWTech::RFSWRobotProcessBasic(){

    if(this->mFswTecDAQData.InitPosDone == false && this->mFswTecDAQData.EndPosDone == false){
        this->mFswTecAdjData.RobotOvn = 100;
        this->mFswTecAdjData.RobotPosZOffsetAbs = 0;
        this->nowRFSStage = InitStage;
    }

    switch (this->nowRFSStage) {
    case InitStage:
        this->RFSWRobotProcessInitStage();
        break;
    case InsertStage:
        this->RFSWRobotProcessInsertStage();
        break;
    case PreWaitStage:
        this->RFSWRobotProcessPreWaitStage();
        break;
    case WeldingStage:
        this->RFSWRobotProcessWeldingStage();
        break;
    case FinisStage:
        this->RFSWRobotProcessFinisStage();
        break;
    default:
        break;
    }
    return 1;
}

short BasicRFSWTech::RFSWRobotProcessInitStage(){
    if(this->mFswTecDAQData.InitPosDone == false){
        this->mFswTecAdjData.RobotOvn = 100;
        this->mFswTecAdjData.RobotPosZOffsetAbs = 0;
       this-> nowRFSStage = InitStage;
    }
    else{
        this->mFswTecAdjData.RobotOvn = 0.1;
        this->mFswTecAdjData.RobotPosZOffsetAbs = 0;
        this->nowRFSStage = InsertStage;
    }
    return 1;
}

short BasicRFSWTech::RFSWRobotProcessInsertStage(){
    if(mFswTecDAQData.ForceSensorReal < mFswTecDAQData.ForceInitPosSet){
        mFswTecAdjData.RobotOvn = 0.1;
        mFswTecAdjData.RobotPosZOffsetAbs = mFswTecAdjData.RobotPosZOffsetAbs + 0.02;
        this->nowRFSStage = InsertStage;
    }
    else{
        mFswTecAdjData.RobotOvn = 0.1;
        mFswTecAdjData.RobotPosZOffsetAbs = mFswTecAdjData.RobotPosZOffsetAbs;
        this->nowRFSStage = PreWaitStage;
        StaticTimeKey = QDateTime::currentDateTime().toMSecsSinceEpoch()/1000.0;
    }
    return 1;
}

short BasicRFSWTech::RFSWRobotProcessPreWaitStage(){
    DynamicTimekey = QDateTime::currentDateTime().toMSecsSinceEpoch()/1000.0;
    if(DynamicTimekey - StaticTimeKey >= (mFswTecDAQData.InitPosDelayTime/1000.0)){
        this->mFswTecAdjData.RobotOvn = 0.1;
        mFswTecAdjData.RobotPosZOffsetAbs = mFswTecAdjData.RobotPosZOffsetAbs;
        this->nowRFSStage = WeldingStage;
    }
    else{
        this->mFswTecAdjData.RobotOvn = 0.1;
        mFswTecAdjData.RobotPosZOffsetAbs = mFswTecAdjData.RobotPosZOffsetAbs;
    }
    return 1;
}

short BasicRFSWTech::RFSWRobotProcessWeldingStage(){
    double ForceOffsetRela = 0;
    if(this->mFswTecDAQData.EndPosDone == false){
        this->mFswTecAdjData.RobotOvn = 100;
        this-> nowRFSStage = WeldingStage;

        if(this->mFswTecDAQData.ForceSensorReal > this->mFswTecDAQData.ForceProcessSet + (this->mFswTecDAQData.ForceProcessSet * 0.08))
            ForceOffsetRela = -5 * this->mFswTecDAQData.ForceAdjGlobalCoefficient;
        else if(this->mFswTecDAQData.ForceSensorReal <= this->mFswTecDAQData.ForceProcessSet +(this->mFswTecDAQData.ForceProcessSet * 0.08) && this->mFswTecDAQData.ForceSensorReal >  this->mFswTecDAQData.ForceProcessSet +(this->mFswTecDAQData.ForceProcessSet * 0.05))
            ForceOffsetRela = -3 * this->mFswTecDAQData.ForceAdjGlobalCoefficient;
        else if(this->mFswTecDAQData.ForceSensorReal <= this->mFswTecDAQData.ForceProcessSet +(this->mFswTecDAQData.ForceProcessSet * 0.05) && this->mFswTecDAQData.ForceSensorReal >  this->mFswTecDAQData.ForceProcessSet +(this->mFswTecDAQData.ForceProcessSet * 0))
            ForceOffsetRela = 0 * this->mFswTecDAQData.ForceAdjGlobalCoefficient;
        else if(this->mFswTecDAQData.ForceSensorReal <= this->mFswTecDAQData.ForceProcessSet +(this->mFswTecDAQData.ForceProcessSet * 0)    && this->mFswTecDAQData.ForceSensorReal >  this->mFswTecDAQData.ForceProcessSet -(this->mFswTecDAQData.ForceProcessSet * 0.05))
            ForceOffsetRela = 0 * this->mFswTecDAQData.ForceAdjGlobalCoefficient;
        else if(this->mFswTecDAQData.ForceSensorReal <= this->mFswTecDAQData.ForceProcessSet -(this->mFswTecDAQData.ForceProcessSet * 0.05) && this->mFswTecDAQData.ForceSensorReal >  this->mFswTecDAQData.ForceProcessSet -(this->mFswTecDAQData.ForceProcessSet * 0.08))
            ForceOffsetRela = 1 * this->mFswTecDAQData.ForceAdjGlobalCoefficient;
        else if(this->mFswTecDAQData.ForceSensorReal <= this->mFswTecDAQData.ForceProcessSet -(this->mFswTecDAQData.ForceProcessSet * 0.08))
            ForceOffsetRela = 1.5 * this->mFswTecDAQData.ForceAdjGlobalCoefficient;
        else
            ForceOffsetRela = 0;
        this->mFswTecAdjData.RobotPosZOffsetAbs = this->mFswTecAdjData.RobotPosZOffsetAbs + ForceOffsetRela;

    }
    else{
        this->mFswTecAdjData.RobotOvn = 100;
        this->mFswTecAdjData.RobotPosZOffsetAbs = 0;
        this->nowRFSStage = FinisStage;
    }
    return 1;
}

short BasicRFSWTech::RFSWRobotProcessFinisStage(){
    if(this->mFswTecDAQData.InitPosDone == false){
        this->mFswTecAdjData.RobotOvn = 100;
        this->mFswTecAdjData.RobotPosZOffsetAbs = 0;
        this-> nowRFSStage = InitStage;
    }
    else{
        this->mFswTecAdjData.RobotOvn = 100;
        this->mFswTecAdjData.RobotPosZOffsetAbs = 0;
        this-> nowRFSStage = FinisStage;
    }
    return 1;
}

short BasicRFSWTech::SetFswAdjData(ST_FswTecAdjData &FswTecAdjData){
    FswTecAdjData = this->mFswTecAdjData;
    return 1;
}
